/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "PoseSpeedEKF.h"


CPoseSpeedEKF::CPoseSpeedEKF(float posx, float posy,float posa, float v, float w,
														 float varx, float vary, float vara, float varv, float varw,
														 float qx, float qy, float qa, float qv, float qw,
														 float rx, float ry, float ra, float rv, float rw,
														 float robot_acceleration, float robot_angular_acceleration){
		
											 
		P.ReSize(5,5);           ///< Covariance
		Q.ReSize(5);             ///< Prediction (noise) Variance
		Rs.ReSize(2);            ///< Speed Measurement (noise) Variance
		Rp.ReSize(3);            ///< Pose Measurement (noise) Variance
		Hs.ReSize(2,5);          ///< Measurement Matrix for speed measurement
		Hp.ReSize(3,5);          ///< Measurement Matrix for pose measurement
		x.ReSize(5);             ///< State
		
		P = 0.0;
		Q = 0.0;
		Rs = 0.0;
		Rp = 0.0;
		Hs = 0.0;
		Hp = 0.0;
		x = 0.0; 
		
		acceleration=robot_acceleration;
		angular_acceleration=robot_angular_acceleration;
		
		Hs << 0 << 0 << 0 << 1 << 0
			 <<	0 << 0 << 0 << 0 << 1;
	
		Hp  << 1 << 0 << 0 << 0 << 0
				<< 0 << 1 << 0 << 0 << 0
				<< 0 << 0 << 1 << 0 << 0;

		///Set initial values
		x(1) = posx; x(2) = posy; x(3) = posa; x(4)= v; x(5) = w;

		P(1,1) = varx; P(2,2) = vary; P(3,3)=vara; P(4,4) = varv; P(5,5) = varw;
		///Prediction noise	
		Q(1,1) = qx; Q(2,2) = qy; Q(3,3) = qa; Q(4,4) = qv; Q(5,5)=qw;
		/// Measurement Noises 
		Rs(1,1)= rv; Rs(2,2) = rw;
		Rp(1,1) = rx; Rp(2,2) = ry; Rp(3,3) = ra;
#ifdef USE_POSE_SPEED_EKF_MAIN
		cout << "Initial State\n" << x;
		cout << "P--------------\n" << P;
		cout << "Hs--------------\n" << Hs;
		cout << "Hp\n" << Hp;
		cout << "Rp--------\n" << Rp;
		cout << "Rs--------\n" << Rs;
		cout << "Q--------\n" << Q;	
#endif		
}
/**
* Initialize the filter state values
**/
void CPoseSpeedEKF::Initialize(float posx, float posy,float posa, float v, float w){
		x(1) = posx; x(2) = posy; x(3) = posa; x(4)= v; x(5) = w;
		P(1,1) = 100; P(2,2) = 100; P(3,3)=100; P(4,4) = 10; P(5,5) = 10;	
}

CPoseSpeedEKF::~CPoseSpeedEKF(){
}

void CPoseSpeedEKF::topi(double &angle){
		if(angle>M_PI) while(angle>M_PI) angle-=2.0*M_PI;
		else if(angle<-M_PI) while(angle<-M_PI) angle+=2.0*M_PI;
}


/**
 * Predict from state k->k+1;
 * @param v Speed Control during the prediction
 * @param w Angular speed control during the prediction
 * @param dt time step in seconds 
 **/
void CPoseSpeedEKF::predict(float v, float w, float dt){
		float kacc,kalpha;
		//if(fabs(v-x(4))<=acceleration*dt){
		//		kacc = 0;
		//}
		//else 
		if(x(4)<v) kacc = 1;
		else if(x(4)>v) kacc = -1;
		else kacc = 0;
		
		//if(fabs(w-x(5))<=angular_acceleration*dt){
		//		kalpha = 0;
		//}
		///else 
		if(x(5)<w) kalpha = 1;
		else if(x(5)>w) kalpha = -1;
		else kalpha = 0;
		
		ColumnVector xPri = x;    ///< State
		/// Predict the state
		xPri(1) += x(4)*dt*cos(x(3));
		xPri(2) += x(4)*dt*sin(x(3));
		xPri(3) += x(5)*dt;
		
		if(fabs(xPri(4))<acceleration*dt && fabs(v)<0.001) xPri(4) = 0;
		else 	xPri(4) += kacc*acceleration*dt;
		if(fabs(xPri(5))<=angular_acceleration*dt && fabs(w)<0.001) xPri(5)=0;
		else xPri(5) += kalpha*angular_acceleration*dt;
		
		
		
		topi(xPri(3));
		x=xPri;
		
		Matrix G(5,5);
		
		df(G,dt);
		
		///Covariance of prediction
		P = G * P * G.t() + Q;
			
}

/**
 * Predict from state k->k+1; Using differential movement from previous frame to this instance
 *
 * @param dl The length of the differential movement
 * @param a  the direction of movement in differential frame [a = atan2((y(k+1)-y(k)),x(k+1)-x(k))]
 * @param dPhii The change in heading  
 **/
void CPoseSpeedEKF::diffPosePredict(float dl, float a, float dPhii){
		ColumnVector xPri = x;    ///< State
		/// Predict the state
		xPri(1) += dl*cos(x(3)+a);
		xPri(2) += dl*sin(x(3)+a);
		xPri(3) += dPhii;
		topi(xPri(3));
		x=xPri;
		
		Matrix G(5,5);
		
		diffPoseDf(G,dl,a);
		
		///Covariance of prediction
		P = G * P * G.t() + Q;
			
}

/**
 * Predict from state k->k+1; Using differential movement from previous frame to this instance
 *
 * @param dl The length of the differential movement
 * @param a  the direction of movement in differential frame [a = atan2((y(k+1)-y(k)),x(k+1)-x(k))]
 * @param dPhii The change in heading  
 **/
void CPoseSpeedEKF::diffPosePredict(float dl, float a, float dPhii, float varx,float vary,float vara){
		ColumnVector xPri = x;    ///< State
		/// Predict the state
		xPri(1) += dl*cos(x(3)+a);
		xPri(2) += dl*sin(x(3)+a);
		xPri(3) += dPhii;
		topi(xPri(3));
		x=xPri;
		
		Matrix G(5,5);
		
		diffPoseDf(G,dl,a);
		Q(1) = varx; Q(2) = vary; Q(3)=vara;
		///Covariance of prediction
		P = G * P * G.t() + Q;
			
}




/**
* Updates the speed measurement 
* @param v speed meassurement
* @param w angular speed measurement
**/
void CPoseSpeedEKF::updateSpeedMeasurement(float v, float w){
		ColumnVector z(2);
		z(1) = v; z(2) = w;
		
		ColumnVector V = z-Hs * x;
		
		Matrix S = Hs * P * Hs.t() + Rs;
		Matrix W = P * Hs.t() * S.i();
		x = x + W * V;
		P = P - W * Hs * P;
		
}

/**
* Updates the speed measurement 
* @param v speed meassurement
* @param w angular speed measurement
* @param rv variance of v
* @param rw variance of w 
**/
void CPoseSpeedEKF::updateSpeedMeasurement(float v, float w, float rv, float rw){
		Rs(1,1)= rv; Rs(2,2) = rw;
		updateSpeedMeasurement(v,w);
}

/**
 * Updates the pose measurement 
 * @param posx X-coordinate
 * @param posy Y-coordinate
 * @param posa Heading
 */
void CPoseSpeedEKF::updatePoseMeasurement(float posx, float posy, float posa){
		ColumnVector z(3);
		z(1) = posx; z(2) = posy; z(3) = posa;
		
		ColumnVector V = z-Hp * x;
		topi(V(3));
		Matrix S = Hp * P * Hp.t() + Rp;
		Matrix W = P * Hp.t() * S.i();
		 
		x = x + W * V;
		P = P - W * Hp * P;
}

/**
 * Updates the pose measurement 
 * @param posx X-coordinate
 * @param posy Y-coordinate
 * @param posa Heading
 * @param rx Variance of x
 * @param ry Variance of y
 * @param ra Variance of a
 */
void CPoseSpeedEKF::updatePoseMeasurement(float posx, float posy, float posa, float rx, float ry, float ra){
		Rp(1,1) = rx; Rp(2,2) = ry; Rp(3,3) = ra;
		updatePoseMeasurement(posx,posy,posa);
}
				
/**
 * Calculate jacobian
 */
void CPoseSpeedEKF::df(Matrix &G, float dt){
		G   << 1 << 0 << 0 << 0 << 0
				<< 0 << 1 << 0 << 0 << 0
				<< 0 << 0 << 1 << 0 << 0
				<< 0 << 0 << 0 << 1 << 0
				<< 0 << 0 << 0 << 0 << 1;
		
		G(1,3) = -x(4)*dt*sin(x(3));
		G(1,4) = dt*cos(x(3));
		G(2,3) = x(4)*dt*cos(x(3));
		G(2,4) = dt*sin(x(3));
		G(3,5) = dt;	
}


/**
 * Calculate jacobian for diff pose
 */
void CPoseSpeedEKF::diffPoseDf(Matrix &G, float dl, float a){
		G   << 1 << 0 << 0 << 0 << 0
						<< 0 << 1 << 0 << 0 << 0
						<< 0 << 0 << 1 << 0 << 0
						<< 0 << 0 << 0 << 1 << 0
						<< 0 << 0 << 0 << 0 << 1;
		
		G(1,3) = -dl*sin(x(3)+a);
		G(1,4) = 0;
		G(2,3) = dl*cos(x(3)+a);
		G(2,4) = 0;
		G(3,5) = 0;	
}



void CPoseSpeedEKF::getState(float &posx, float &posy, float &posa, float &v, float &w){
		posx = x(1); posy=x(2);posa=x(3); v = x(4); w = x(5); 
}

void CPoseSpeedEKF::getStateVariances(float &vx, float &vy, float &va, float &vv, float &vw){
		vx = P(1,1); vy = P(2,2); va = P(3,3); vv = P(4,4); vw = P(5,5);
}

#ifdef USE_POSE_SPEED_EKF_MAIN
float randomi(float std){
		return (std*((float)rand()/(float)RAND_MAX-0.5));
}

int main(){
 CPoseSpeedEKF ekf(0,0,0,0,0,
									10,10,10,10,10,
									0.01,0.01,0.01,0.05,0.05,
									1.0,1.0,0.2,0.05,0.05,
									0.4,0.4);
 
 int cnt=0;
 float t=0;
 float dt = 0.1;
 float x,y,a,v,w;
 srand(time(NULL));
 while(1){
		float uv = 0.5;
		float uw = sin(t);
		
		ekf.predict(uv,uw,dt);		
		ekf.getState(x,y,a,v,w);		
		ekf.updateSpeedMeasurement(v+randomi(0.05),w+randomi(0.05));
		
		t +=dt;
		
		if(cnt%20==0){
				ekf.getState(x,y,a,v,w);
				ekf.updatePoseMeasurement(x+randomi(0.5),y+randomi(0.5),a+randomi(0.05));
		}
		
		fprintf(stderr,"Estimate (v,w)=(%.2f,%.2f)  (x,y,a)=(%.2f,%.2f,%.2f)\n",v,w,x,y,a);
		
		cnt++;
 }
 
 
 return 0;
}
#endif
